#include "algo_adc.h"

/**
 * 完善采样滤波库，方便系统级使用
 * 20231225
 * 目前包含：
 * 1.一阶互补滤波
 * 2.中值滤波算法
 * 3.算术平均值滤波
 * 4.平滑均值滤波
 * 5.限幅平均滤波
 * 6.卡尔曼滤波 
*/
//基于STM32的ADC采样及各式滤波实现（HAL库，含VOFA+教程）
//https://blog.csdn.net/black_sneak/article/details/129629485
/******ADC几个比较重要的参数：******************
（1）测量范围：测量范围对于 ADC 来说就好比尺子的量程，ADC 测量范围决定了
你外接的设备其信号输出电压范围，不能超过 ADC 的测量范围（比如，STM32系列
的 ADC 正常就不能超过3.3V）。
（2）分辨率：假如 ADC 的测量范围为 0-5V，分辨率设置为12位，那么我们能测
出来的最小电压就是 5V除以 2 的 12 次方，也就是 5/4096=0.00122V。很明显
，分辨率越高，采集到的信号越精确，所以分辨率是衡量 ADC 的一个重要指标。
（3）采样时间：当 ADC 在某时刻采集外部电压信号的时候，此时外部的信号应该
保持不变，但实际上外部的信号是不停变化的。所以在 ADC 内部有一个保持电路，
保持某一时刻的外部信号，这样 ADC 就可以稳定采集了，保持这个信号的时间就是
采样时间。
（4）采样率：也就是在一秒的时间内采集多少次。很明显，采样率越高越好，当采
样率不够的时候可能会丢失部分信息，所以 ADC 采样率是衡量 ADC 性能的另一个
重要指标（详细参考信号处理方向书籍）。

特别说明： 
ADC 是12位逐次逼近型模数转换器，输出数值范围是 0 ~ 2^12 -1（0 ~ 4095），
满量程是 3.3V ，分辨率就是最低有效位（LSB）的对应输入电压值。
分辨率 =3300/4095 ≈ 0.806mV。
*************************************************/
/*************************************
 * 一阶互补滤波
 * 方法：取a=0~1,本次滤波结果=（1-a）本次采样值+a上次滤波结果
 * 优点：对周期性干扰具有良好的抑制作用适用于波动频率较高的场合
 * 缺点：相位滞后，灵敏度低滞后程度取决于a值大小不能消除滤波频率高于采样频率的1/2的干扰信号
 * 一阶互补滤波的局限性还是很大的，效果非常一般。
 * ***********************************/
//一阶互补滤波
int firstOrderFilter(int newValue, int oldValue, float a)
{
	return a * newValue + (1-a) * oldValue;
}
// ADC_value=HAL_ADC_GetValue(&hadc1);			//获取ADC1的数值
// //主函数
// while(1)
// {
//     HAL_ADC_Start(&hadc1);						//开启ADC1，放置在while循环中
//     Filtering_Value = firstOrderFilter(HAL_ADC_GetValue(&hadc1),ADC_value,0.3);    //滤波算法
// 	HAL_Delay(10);								//延迟函数，防止采样失效
// 	printf("ADC_value:%d\n", ADC_value);
// }
/*************************************
 * 中值滤波算法
 * 方法：连续采样N次（N取奇数）把N次采样值按大小排列取中间值为本次有效值
 * 优点：能有效克服因偶然因素引起的波动干扰；对温度、液位等变化缓慢的被测参数有良好的滤波效果
 * 缺点：对流量，速度等快速变化的参数不宜
 * 中值滤波对消除异常值和平稳化AD采样都具有十分有效的结果。
 ************************************/
//中值滤波算法
int middleValueFilter(int N)
{
    int value_buf[N];
    int i,j,k,temp;
    for( i = 0; i < N; ++i)
    {
        value_buf[i] = HAL_ADC_GetValue(&hadc1);					
    }
    for(j = 0 ; j < N-1; ++j)
    {
        for(k = 0; k < N-j-1; ++k)
        {
            //从小到大排序，冒泡法排序
            if(value_buf[k] > value_buf[k+1])
            {
                temp = value_buf[k];
                value_buf[k] = value_buf[k+1];
                value_buf[k+1] = temp;
            }
        }
    }
		
    return value_buf[(N-1)/2];
}
/*************************************
 * 算术平均滤波
 * 方法：连续取N个采样值进行算术平均运算;
 * N值较大时：信号平滑度较高，但灵敏度较低
 * N值较小时：信号平滑度较低，但灵敏度较高
 * N值的选取：一般流量，N=12；压力：N=4
 * 优点：试用于对一般具有随机干扰的信号进行滤波。这种信号的特点是有一个平均值，信号在某一数值范围附近上下波动。
 * 缺点：测量速度较慢或要求数据计算较快的实时控制不适用。
 * 算术平均滤波表现出了一定的平稳性，同时具有波动的伴随性（合理选择N值可能达到很好的效果）。
 ************************************/
//算术平均值滤波
int averageFilter(int N)
{
   int sum = 0;
   short i;
   for(i = 0; i < N; ++i)
   {
        sum += HAL_ADC_GetValue(&hadc1);	
   }
   return sum/N;
}
/*************************************
 * 滑动平均滤波
 * 方法：把连续取N个采样值看成一个队列，队列的长度固定为N。每次采样到一个新数据放入队尾，
 *       并扔掉原来队首的一次数据(先进先出原则)。把队列中的N个数据进行算术平均运算,就可获得新的滤波结果。
 * N值的选取：流量，N=12；压力：N=4；液面，N=4~12；温度，N=1~4
 * 优点：对周期性干扰有良好的抑制作用，平滑度高；试用于高频振荡的系统
 * 缺点：灵敏度低；对偶然出现的脉冲性干扰的抑制作用较差，不适于脉冲干扰较严重的场合
 *      比较浪费RAM（改进方法，减去的不是队首的值，而是上一次得到的平均值）
 * 平滑均值滤波相较于普通的算术平均滤波，突出一个平滑特性。
 * 可以从上述VOFA+的波形图看出，平滑滤波可以有效抵消AD采样的刺噪并稳定化采集
 * （据作者同门实战反应平滑滤波的效果还是非常好的，尤其在控制方面）。
 ************************************/
//平滑均值滤波
#define N 10
int value_buf[N];
int sum=0;
int curNum=0;
int moveAverageFilter()
{
    if(curNum < N)
    {
        value_buf[curNum] = HAL_ADC_GetValue(&hadc1);
        sum += value_buf[curNum];
			  curNum++;
        return sum/curNum;
    }
    else
    {
        sum -= sum/N;
        sum += HAL_ADC_GetValue(&hadc1);
        return sum/N;
    }
}
/*************************************
 * 限幅平均滤波
 * 方法：相当于“限幅滤波法”+“递推平均滤波法”
 * 每次采样到的新数据先进行限幅处理再送入队列进行递推平均滤波处理
 * 优点：对于偶然出现的脉冲性干扰，可消除有其引起的采样值偏差。
 * 缺点：比较浪费RAM
 ************************************/
//限幅平均滤波
#define A 50        //限制幅度阈值
#define M 12
int data[M];
int First_flag=0;
int LAverageFilter()
{
  int i;
  int temp,sum,flag=0;
  data[0]=HAL_ADC_GetValue(&hadc1);
	for(i=1;i<M;i++)
	{
		temp=HAL_ADC_GetValue(&hadc1);
		if((temp-data[i-1])>A||((data[i-1]-temp)>A))
		{
		  i--;flag++;
		}
		else
		{
			data[i]=temp;
		}
	}
  for(i=0;i<M;i++)
  {
    sum+=data[i];
  } 
  return  sum/M;
}
/*************************************
 * 卡尔曼滤波 
核心思想：根据当前的仪器"测量值" 和上一刻的 “预测量” 和 “误差”，计算得到当前的最优量，
        再预测下一刻的量。里面比较突出的是观点是：把误差纳入计算，而且分为预测误差和
        测量误差两种，通称为噪声。还有一个非常大的特点是：误差独立存在，始终不受测量数据的影响。
优点：巧妙的融合了观测数据与估计数据，对误差进行闭环管理，将误差限定在一定范围。
      适用性范围很广，时效性和效果都很优秀。
缺点：需要调参，参数的大小对滤波的效果影响较大。
作者本人是非常喜欢卡尔曼滤波的，VOFA+显示的波形图开源看出卡尔曼滤波有一定的去噪稳定特性的，虽然效果不是特别优秀。
卡尔曼滤波的普适性很强，尤其在控制与多传感器融合方向，只要参数调整的好，效果出奇优秀。
 ************************************/
//卡尔曼滤波
int KalmanFilter(int inData)
{
		static float prevData = 0;                                 //先前数值
		static float p = 10, q = 0.001, r = 0.001, kGain = 0;      // q控制误差  r控制响应速度 
	
		p = p + q;
		kGain = p / ( p + r );                                     //计算卡尔曼增益
		inData = prevData + ( kGain * ( inData - prevData ) );     //计算本次滤波估计值
		p = ( 1 - kGain ) * p;                                     //更新测量方差
		prevData = inData;
		return inData;                                             //返回滤波值
}
